//     Subscribers: /initialpose: geometry_msgs/msg/PoseWithCovarianceStamped

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_msgs/srv/set_initial_pose.hpp"

using nav2_msgs::srv::SetInitialPose;
using namespace std::chrono_literals;

class InitRobotPose : public rclcpp::Node
{
private:
    rclcpp::Client<SetInitialPose>::SharedPtr client_;

public:
    InitRobotPose() : Node("initial_pose_setter_node")
    {
        client_ = this->create_client<SetInitialPose>("/set_initial_pose");

        // 等待服务器响应
        while (!client_->wait_for_service(5s))
        {
            RCLCPP_INFO(this->get_logger(), "等待服务器set_initial_pose响应！");
        }

        // 准备请求
        auto request = std::make_shared<nav2_msgs::srv::SetInitialPose::Request>();
        request->pose.header.frame_id = "map";
        request->pose.header.stamp = this->get_clock()->now();
        request->pose.pose.pose.position.x = 0.0;
        request->pose.pose.pose.position.y = 0.0;
        request->pose.pose.pose.orientation.w = 1.0;

        // 发送请求
        auto future = client_->async_send_request(request);

        try
        {
            auto response = rclcpp::spin_until_future_complete(this->get_node_base_interface(),future);
            if (response == rclcpp::FutureReturnCode())
            {
                RCLCPP_INFO(this->get_logger(), "初始化位姿成功!");
            }
            else
            {
                RCLCPP_INFO(this->get_logger(), "初始化位姿失败!");
            }
        }
        catch (const std::exception &e)
        {
            RCLCPP_ERROR(this->get_logger(), "呼叫服务失败：%s！", e.what());
        }
    }
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<InitRobotPose>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
